
%% Init
clc;close;clear
addpath("./M_Function/")
hold all
%% Plant
s = tf('s')
% R = 1; % Ω
% L = 1e-3; % H

% Motor_tf = 1/(L*s+R);

Motor_w = 1000;
Motor_T = 1 / Motor_w;
Motor_tf = 1/(Motor_T*s+1)


freq_rads = (1:0.1:1000)*2*pi;
fs_rads = freq_rads(end)*s
ts = 2*pi/fs_rads;

% margin(Motor_tf)
Motor_frd = tf2frd(Motor_tf, freq_rads, fs_rads)

%% PID
kp = 1;
wi = 10; % 积分环节转折频率
wd = 100; % 微分环节转折频率
PI_Loop = kp*(1 + wi/s);
PD_Loop = kp*(1 + s/wd);
PID_Loop = kp*(1 + wi/s + s/wd);
figure(2)
bode(PI_Loop)
bode(PD_Loop)
bode(PID_Loop)
legend("PI Loop","PD Loop","PID Loop")